#include <pthread.h>
#include <opencv2/opencv.hpp>

#include "include/thread_mat.h"
#include "include/thread_disp.h"
#include "include/thread_gestrec.h"
#include "../wiringPi/wiringPi.h"

class light {
public:
    static void light_on(int pin) {
        pinMode(pin, OUTPUT);
        digitalWrite(pin, HIGH);
    }

    static void light_off(int pin) {
        digitalWrite(pin, LOW);
    }
};

int main() {
    light::light_on(29);
    thread_mat l(3), r(4);
    l.run();
    r.run();
    thread_disp disp(l, r);
    disp.run();
    while (true) {
        pthread_rwlock_rdlock(&disp.rwlock_disp);
        if (disp.id_disp_ready)
            break;
        pthread_rwlock_unlock(&disp.rwlock_disp);
    }
    thread_gestrec gestrec(r, l);
    gestrec.run();

    cv::namedWindow("raw_l");
    cv::namedWindow("raw_r");
    cv::namedWindow("disp8");

    cv::namedWindow("num_finger");
    cv::Mat num_finger(100, 100, CV_8U);

    while (true) {
        pthread_rwlock_rdlock(&l.raw.rwlock_raw);
        cv::imshow("raw_l", l.raw.mat_raw);
        pthread_rwlock_unlock(&l.raw.rwlock_raw);

        pthread_rwlock_rdlock(&r.raw.rwlock_raw);
        cv::imshow("raw_r", r.raw.mat_raw);
        pthread_rwlock_unlock(&r.raw.rwlock_raw);

        pthread_rwlock_rdlock(&disp.rwlock_disp);
        cv::imshow("disp8", disp.disp8);
        pthread_rwlock_unlock(&disp.rwlock_disp);

        pthread_rwlock_rdlock(&gestrec.rwlock);
        int num = (int) gestrec.result;
        cv::putText(num_finger, std::to_string(num), cv::Point(0, 0), cv::FONT_HERSHEY_PLAIN, 10,
                    cv::Scalar(0, 255, 0));
        imshow("num_finger", num_finger);
        pthread_rwlock_unlock(&gestrec.rwlock);
        cv::waitKey(30);
    }
}